#include "Bridge.h"
#include "Spring.h"
#include "TorsionSpring.h"
#include <cstdio>

/*
 * Notes on rigidity:
 * As the stiffness goes up, numIter has to go up too, otherwise the system
 * becomes unstable and literally blows up.
 * With timeStep = 0.015, gravity = -9.81, damping = 20
 * 	numIter 	stiffness
 * 	3		1.7
 * 	10		1.875
 * 	100		1.98828125
 */

Bridge::Bridge(ParticleSystem* _particleSystem) :
	Object(_particleSystem)
{
	/*
	Vector3 position(-0.5, 0, 0);
	double length = 0.1;
	double invMass = 1;

	Particle* p1;
	Particle* p2;

	particle.insert(p0 = p1 = new Particle(position, 0));
	for(int i = 0; i < 10; ++i) {
		position.x += length;
		//position.y += length;
		//invMass += invMass/2;
		particle.insert(p2 = new Particle(position, invMass));
		constraint.insert(new Spring(p1, p2, 1.875));
		p1 = p2;
	}
	pN = p1;
	pN->invMass = 0;
	*/

	/*
	Vector3 pos(-0.5, 0, 0);
	double l = 0.1;
	for(int i = 0; i < 10; ++i) {
		p[i] = new Particle(pos, 1);
		particle.insert(p[i]);
		if(i > 0) {
			constraint.insert(new Spring(p[i-1], p[i], 1));
		}
		pos.x += l;
	}
	p[0]->invMass = p[9]->invMass = 0;
	*/

	Vector3 pos(-0.5, 0, 0);
	double l = 0.2;
	double im = 1;
	for(int i = 0; i < 16; ++i) {
		particle.insert(p[2*i] = new Particle(pos, im));
		particle.insert(p[2*i+1] = new Particle(pos+Vector3(0, 0, l), im));
		constraint.insert(new Spring(p[2*i], p[2*i+1], 0.3));
		if(i > 0) {
			triangle.insert(new Triangle(p[2*i-2], p[2*i-1], p[2*i]));
			triangle.insert(new Triangle(p[2*i-1], p[2*i], p[2*i+1]));
			constraint.insert(new Spring(p[2*i-2], p[2*i], 1));
			constraint.insert(new Spring(p[2*i-1], p[2*i+1], 1));
			constraint.insert(new Spring(p[2*i-2], p[2*i+1], 1));
			constraint.insert(new Spring(p[2*i-1], p[2*i], 1));
		}
		pos.x += l;
	}
	p[0]->invMass = p[1]->invMass = 0;
	p[2]->invMass = p[3]->invMass = 0;
	p[14]->invMass = p[15]->invMass = 0;

	/*
	Vector3 pos(-0.5, 0, 0);
	particle.insert(p[0] = new Particle(pos, 0));
	particle.insert(p[1] = new Particle(pos+Vector3(0, 0, l), 0));
	particle.insert(p[2] = new Particle(pos+Vector3(l, 0, 0), im));
	particle.insert(p[3] = new Particle(pos+Vector3(l, 0, l), im));
	particle.insert(p[4] = new Particle(pos+Vector3(2*l, 0, 0), im));
	particle.insert(p[5] = new Particle(pos+Vector3(2*l, 0, l), im));
	particle.insert(p[6] = new Particle(pos+Vector3(3*l, 0, 0), 0));
	particle.insert(p[7] = new Particle(pos+Vector3(3*l, 0, l), 0));
	for(int i = 0; i < 7; ++i)
		constraint.insert(new Spring(p[i], p[i+1], 1));
	for(int i = 0; i < 6; ++i)
		constraint.insert(new Spring(p[i], p[i+2], 1));
	for(int i = 0; i <= 4; i += 2)
		constraint.insert(new Spring(p[i], p[i+3], 1));
	constraint.insert(new TorsionSpring(p[0], p[3], p[1], p[2], 0.05));
	*/
	//constraint.insert(new TorsionSpring(p[0], p[3], p[1], p[2]));
}

void Bridge::accumulateForces()
{
	//p0->acceleration = Vector3();
	//pN->acceleration = Vector3();
	p[0]->acceleration = Vector3();
	p[1]->acceleration = Vector3();
	p[2]->acceleration = Vector3();
	p[3]->acceleration = Vector3();
	p[14]->acceleration = Vector3();
	p[15]->acceleration = Vector3();
}
